/*
 * Sensor acquisition tasks
 *
 * These tasks have the job of reading the sensors while everything else
 * is going on.
 *
 * by Zik Saleeba 2008-04-28
 */

#include "ffox.h"
#include "AT91SAM7X256.h" 
#include "lib_AT91SAM7X256.h"
#include "hp03.h"
#include "analogin.h"
#include "adc.h"
#include "sensor.h"
#include "dbgprintf.h"

extern int AnalogCount;
extern int AltimeterCount;

/* macros */
#define ANALOG_HZ         250         /* take 250 samples per second */
#define LED_HZ             16

/* module variables */
volatile float   Sensor_Altitude;
volatile float   Sensor_Temperature;
volatile int     Sensor_AltitudeUpdated;

enum Sensor_LEDState Sensor_CurrentLEDState = Sensor_LEDStateNoGPS;


/* local prototypes */
static portTASK_FUNCTION_PROTO(Altimeter_Task, pvParameters);
static portTASK_FUNCTION_PROTO(Analog_Task, pvParameters);
static portTASK_FUNCTION_PROTO(LED_Task, pvParameters);
static portTASK_FUNCTION_PROTO(Hello_Task, pvParameters);


/*
 * NAME:        Sensor_StartTasks
 * ACTION:      Create the tasks which will read all the sensors and
 *              make these readings available to the rest of the system.
 * PARAMETERS:  unsigned portBASE_TYPE uxPriority - the priority to
 *                  start the tasks at
 */

void Sensor_StartTasks(unsigned portBASE_TYPE uxPriority)
{
    FFox_TaskCreate(LED_Task, ( const signed portCHAR * const ) "led ", MEDIUM_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    //FFox_TaskCreate(Hello_Task, ( const signed portCHAR * const ) "helo", MEDIUM_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    FFox_TaskCreate(Altimeter_Task, (const signed portCHAR * const) "alti", MEDIUM_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);
    FFox_TaskCreate(Analog_Task, (const signed portCHAR * const) "alog", MEDIUM_STACK_SIZE, NULL, uxPriority, (xTaskHandle *)NULL);

    /* initialise the sensor values */
    Sensor_Altitude = 0.0;
    Sensor_Temperature = 0.0;
    Sensor_AltitudeUpdated = false;
}


/*
 * NAME:        LED_Task
 * ACTION:      A task which flashes a LED
 */
 
static portTASK_FUNCTION(LED_Task, pvParameters)
{
    /*
     * NoGPS       - slow red/green
     * Calibrating - slow red red/green green
     * Active      - green flash on/off
     * Manual      - quick green flash, long black
     * FatalError  - red red red / black
     */
    static unsigned long RedPattern[Sensor_LEDStateMax] = 
        { 0xff00ff00, 0x00ff00ff, 0x00800080, 0x00000000, 0x00000000, 0x88800000 };
    static unsigned long GreenPattern[Sensor_LEDStateMax] = 
        { 0xff00ff00, 0xff00ff00, 0xff00ff00, 0xfffffffe, 0x80000000, 0x00000000 };

	int Magic1 = STACK_MAGIC;
	int LEDBit = 31;
	portTickType PreviousWakeTime;

	/* The parameters are not used. */
	(void)pvParameters;

	/* get the current time */
    PreviousWakeTime = xTaskGetTickCount();
	
	while (true)
	{
		/* set the LEDs */
	    if ((1UL << LEDBit) & RedPattern[Sensor_CurrentLEDState])
	        AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, LED1);
	    else
	        AT91F_PIO_SetOutput(AT91C_BASE_PIOA, LED1);
	        
	    if ((1UL << LEDBit) & GreenPattern[Sensor_CurrentLEDState])
	        AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, LED2);
	    else
	        AT91F_PIO_SetOutput(AT91C_BASE_PIOA, LED2);
		
		/* move to the next LED bit */
		LEDBit--;
		if (LEDBit < 0)
		    LEDBit = 31;

        if (Magic1 != STACK_MAGIC)
            dbgprintf("ERROR: LED stack corrupted\r\n");
		
		/* wait for a smidge */
		vTaskDelayUntil(&PreviousWakeTime, 1000/LED_HZ/portTICK_RATE_MS);
	}
}


/*
 * NAME:        Hello_Task
 * ACTION:      A task which outputs "hello" on the serial port
 */
 
static portTASK_FUNCTION(Hello_Task, pvParameters)
{
	int Magic1 = STACK_MAGIC;

	/* The parameters are not used. */
	(void)pvParameters;
	
	while (true)
	{
		
		/* send a message on the serial port */
		dbgprintf("Hello\r\n");
		
		/* wait for a second */
		vTaskDelay(1000/portTICK_RATE_MS);

        if (Magic1 != STACK_MAGIC)
            dbgprintf("ERROR: Hello stack corrupted\r\n");
	}
}


/*
 * NAME:        Altimeter_Task
 * ACTION:      Continuously reads from the HP03 barometric pressure
 *              sensor, which gives us altitude and temperature. This
 *              updates approximately 10 times a second.
 */

static portTASK_FUNCTION(Altimeter_Task, pvParameters)
{
	int Magic1 = STACK_MAGIC;
	double TempTemp;
	int Magic2 = STACK_MAGIC;
	
	/* The parameters are not used. */
    (void)pvParameters;

    while (true)
    {
        /* get the altitude and temperature */
        Sensor_Altitude = HP03_GetAltitude(&TempTemp);
        Sensor_Temperature = TempTemp;
        Sensor_AltitudeUpdated = true;
        
#if 0
		/* send a message on the serial port */
		dbgprintf("Altitude=");
		dbgprintd(Sensor_Altitude);
		dbgprintf(",");
		dbgprintd(Sensor_Temperature);
		dbgprintf("\r\n");
#endif
        AltimeterCount++;
        
        if (Magic1 != STACK_MAGIC || Magic2 != STACK_MAGIC)
            dbgprintf("ERROR: altimeter stack corrupted\r\n");
    }
}


/*
 * NAME:        Analog_Task
 * ACTION:      Continuously reads the analog inputs, including
 *              the thermopiles, gyros and accelerometers. This
 *              updates 250 times a second.
 */

static portTASK_FUNCTION(Analog_Task, pvParameters)
{
	portTickType PreviousWakeTime;
	int Channel;

	/* The parameters are not used. */
    (void)pvParameters;

	/* get the current time */
    PreviousWakeTime = xTaskGetTickCount();
    
    while (true)
    {
        /* get all the analog inputs */
        for (Channel = 0; Channel < NUM_ANALOG_INPUTS; Channel++)
            adc_addvalue(Channel, AnalogIn_GetValue(Channel));

        AnalogCount++;
        
	    /* wait until the loop period has passed */
	    vTaskDelayUntil(&PreviousWakeTime, 1000/ANALOG_HZ/portTICK_RATE_MS);
    }
}

